#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include <util/delay.h>

#include "utils.h"
#include "oi.h"
#include "sonar.h"

enum {PHANTOM_SONG};

volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];
volatile uint8_t sensors[Sen6Size];

volatile int16_t distance = 0;
volatile int16_t angle = 0;

//void delayAndUpdateSensors(unsigned int time_ms);
void initialize(void);
void drive(int16_t velocity, int16_t radius);

void playSong(uint8_t songnum);
void defineSongs(void);

int main (void) 
{
    // Set up Create and module
    initialize();
    powerOnCreate();
    serialTx(CmdStart);
    baud(Baud57600);
    serialTx(CmdControl);
    serialTx(CmdFull);
    
    delayMs(2000);

    /**************************************************************************
     * Set voltage for Low-Side Driver 2
     **************************************************************************/
    serialTx(144); // pwm low-side drivers opcode
    serialTx(37);   // LD2 at 37% (4.8-6V)
    serialTx(0);   // LD1 off
    serialTx(0);  // LD0 off

    defineSongs();

    // Stop just as a precaution
    drive(0, RadStraight);

    playSong(PHANTOM_SONG);

    int16_t toDo = 32;
    int8_t cur = 0;
    while (toDo) {
        --toDo;
        if (cur > 7) {
            cur = 0;
        }
        ledsSet(1 << cur);
        delayMs(50);
        ++cur;
    }
    
    toDo = 64;
    int8_t dir = 1;
    cur = 0;
    while (toDo) {
        --toDo;
        ledsSet(1 << cur);
        delayMs(50);
        
        cur += dir;
        if (cur > 7) {
            cur = 6;
            dir = -dir;
        } else if (cur < 0) {
            cur = 1;
            dir = -dir;
        }
    }

    ledsSet(0);

    int8_t deg = -90;
    int8_t increm = 10;

    int8_t binMode = 0;
    int8_t ubPressed = 0;
    
    setSerialDestination(UART_TO_USB);
    while (1) {
        if (UserButtonPressed) {
            if (!ubPressed) {
                ubPressed = 1;
                binMode = 1 - binMode;
            }
        } else {
            ubPressed = 0;
        }
        
        if (binMode) {
            LED2Off;
        } else {
            LED2On;
        }
        
        deg += increm;
        if (deg >= 90  || deg <= -90) {
            increm = -increm;
        }

        servoRotate(deg);
        delayMs(100);
        
        sonarStart(0xE0);
        sonarStart(0xE2);
        delayMs(100);
        int16_t dist0 = sonarRead(0xE0);
        int16_t dist2 = sonarRead(0xE2);

        debugWriteInt(dist0, UART_TO_USB);
        debugWriteChar(' ', UART_TO_USB);
        debugWriteInt(dist2, UART_TO_USB);
        debugWriteChar(13, UART_TO_USB);
        //delayMs(50);
        //setSerialDestination(UART_TO_USB);
        //delayMs(200);
        
        if (dist0 == -1) {
            LED1On;
        } else {
            if (binMode) {
                ledsSet(dist0 & 0xFF);
            } else {
                // level mode
                int16_t lev = dist0/6;
                if (lev > 8) {
                    lev = 8;
                }
                ledsSet(((1 << lev)-1) & 0xFF);
            }
        }
       
    }
}

/******************************************************************************
 * Initializes ATmega168: I/O pins, timers, UART, etc.
 ******************************************************************************/
void initialize(void)
{
    cli(); // Disable interrupts

    /**************************************************************************
     * Set I/O pins
     **************************************************************************/
    DDRB = 0x10;
    PORTB = 0xCF;
    DDRC = 0x3F;//0x00;
    PORTC = 0xFF;
    DDRD = 0xE6;
    PORTD = 0x7D;

    /**************************************************************************
     * Initialize LEDs
     **************************************************************************/
     ledsInit();

    /**************************************************************************
     * Set up Timer 0 to generate an interrupt every 1 ms
     **************************************************************************/
    TCCR0A = _BV(WGM01);    // CTC mode
    TCCR0B = _BV(CS02);     // prescaler = 8
    OCR0A = (F_CPU / (256 * 1000UL)) - 1; // (F_CPU / (256 * frequency)) - 1;
    TIMSK0 = _BV(OCIE0A);   // generate interrupt

    /**************************************************************************
     * Set up Timer 1 to generate a 20ms (50Hz) steps with regulated pulse.
     * This is for servo control.
     **************************************************************************/
    TCCR1A = _BV(COM1A1);
    TCCR1B = (_BV(WGM13) | _BV(CS11));
    TCCR1C = 0;
    TIMSK1 = 0;
    ICR1 = F_CPU / (2L * 8  * 50); // F_CPU / (2 * Prescaler * DesiredFrequency)
    servoRotate(0);


    /**************************************************************************
     * Initialize TWI (I2C) bus
     **************************************************************************/
    TWSR = 0;
    TWBR = (F_CPU / 100000UL - 16) / 2;

    /**************************************************************************
     * Set up the serial port with rx interrupt
     **************************************************************************/
    //UBRR0 = (F_CPU / (256 * 57600UL)) - 1; // (F_CPU / (256 * baud_rate)) - 1;
    //UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
    //UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));
    UBRR0 = 19;
    UCSR0B = 0x18;
    UCSR0C = 0x06;

    sei(); // Enable interrupts
}


// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
  //uint8_t temp;

  //temp = UDR0;

  //if(sensors_flag)
  //{
//    sensors_in[sensors_index++] = temp;
    //if(sensors_index >= Sen6Size)
//      sensors_flag = 0;
  //}
}


// Delay for the specified time in ms and update sensor values
/*
void delayAndUpdateSensors(uint16_t timeMs)
{
  uint8_t temp;

  timerOn = 1;
  timerCnt = timeMs;
  while(timerOn)
  {
    if(!sensors_flag)
    {
      for(temp = 0; temp < Sen6Size; temp++)
        sensors[temp] = sensors_in[temp];

      // Update running totals of distance and angle
      distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
      angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);

      serialTx(CmdSensors);
      serialTx(6);
      sensors_index = 0;
      sensors_flag = 1;
    }
  }
}
*/
// Send Create drive commands in terms of velocity and radius
void drive(int16_t velocity, int16_t radius)
{
  serialTx(CmdDrive);
  serialTx((uint8_t)((velocity >> 8) & 0x00FF));
  serialTx((uint8_t)(velocity & 0x00FF));
  serialTx((uint8_t)((radius >> 8) & 0x00FF));
  serialTx((uint8_t)(radius & 0x00FF));
}

/*********************************
playSong
*********************************/
void playSong(uint8_t songnum)
{
	serialTx(CmdPlay);
  serialTx(songnum);		
}

/*********************************
defineSongs
*********************************/
// Define songs to be played later
void defineSongs(void)
{
	// The Phantom
	serialTx(CmdSong);
	serialTx(PHANTOM_SONG);
	serialTx(14);
	serialTx(69);
	serialTx(128);
	serialTx(69);
	serialTx(16);
	serialTx(68);
	serialTx(16);
	serialTx(67);
	serialTx(16);
	serialTx(66);
	serialTx(16);
	serialTx(65);
	serialTx(128);
	
	serialTx(63);
	serialTx(16);
	serialTx(65);
	serialTx(16);
	serialTx(1);
	serialTx(32);
	
	serialTx(65);
	serialTx(16);
	serialTx(66);
	serialTx(16);
	serialTx(67);
	serialTx(16);
	serialTx(68);
	serialTx(16);
	serialTx(69);
	serialTx(128);
}

